#include "motor.h"
#include "mymath.h"
#include "zf_pwm.h"

/**
 * @brief          智能车底盘电机初始化
 * @param[in]      none
 * @param[out]     none
 * @retval         none
 */
void Motor_Init(void)
{
    pwm_init(PWMA_CH1P_P60, 17000, 0);
    pwm_init(PWMA_CH2P_P62, 17000, 0);
    pwm_init(PWMA_CH3P_P64, 17000, 0);
    pwm_init(PWMA_CH4P_P66, 17000, 0);
}

/**
 * @brief          设置车子左右两只电机的PWM占空比，范围正负10000
 * @param[in]      _M1Var：车左边的电机
 * @param[in]      _M2Var：车右边的电机
 * @param[out]     none
 * @retval         none
 */
void Motor_SetPWM(int16 _M1Var, int16 _M2Var)
{
    static uint8 s_MLPrevDire = 1; // 0向后，1向前
    static uint8 s_MRPrevDire = 1; // 0向后，1向前

    if (_M1Var >= 0)
    {
        if (s_MLPrevDire)
        { // 这里在输出电机PWM之前先访问一下之前的偏转方向
            // 还得限制住最大值，免得发疯烧坏电机或者驱动
            pwm_duty(PWMA_CH1P_P60, LIMIT_P(_M1Var, MOTOR_MAX_PWM));
            pwm_duty(PWMA_CH2P_P62, 0);
        }
        else
        {
            pwm_duty(PWMA_CH1P_P60, 0);
            pwm_duty(PWMA_CH2P_P62, 0);
        }
        s_MLPrevDire = 1;
    }
    else
    {
        if (s_MLPrevDire)
        {
            pwm_duty(PWMA_CH1P_P60, 0);
            pwm_duty(PWMA_CH2P_P62, 0);
        }
        else
        {
            pwm_duty(PWMA_CH1P_P60, 0);
            pwm_duty(PWMA_CH2P_P62, LIMIT_P(-_M1Var, MOTOR_MAX_PWM));
        }
        s_MLPrevDire = 0;
    }

    if (_M2Var >= 0)
    {
        if (s_MRPrevDire)
        { // 这里在输出电机PWM之前先访问一下之前的偏转方向
            pwm_duty(PWMA_CH3P_P64, LIMIT_P(_M2Var, MOTOR_MAX_PWM));
            pwm_duty(PWMA_CH4P_P66, 0);
            
        }
        else
        {
            pwm_duty(PWMA_CH3P_P64, 0);
            pwm_duty(PWMA_CH4P_P66, 0);
        }
        s_MRPrevDire = 1;
    }
    else
    {
        if (s_MRPrevDire)
        {
            pwm_duty(PWMA_CH3P_P64, 0);
            pwm_duty(PWMA_CH4P_P66, 0);
        }
        else
        {
            pwm_duty(PWMA_CH3P_P64, 0);
            pwm_duty(PWMA_CH4P_P66, LIMIT_P(-_M2Var, MOTOR_MAX_PWM));
        }
        s_MRPrevDire = 0;
    }
}

void BLDC_Init(void)
{
    // 300hz的话1ms是3%占空比，6%是2ms满速
    // 50hz的话转速为零占空比5%,半速7.5%,满速10%
    pwm_init(PWMB_CH1_P74, 50, 500);
    pwm_init(PWMB_CH2_P75, 50, 500);
}

/**
 * @brief          设置车子左右两只无刷电机的PWM占空比，范围0~100
 * @param[in]      _M1Var：车左边的无刷电机
 * @param[in]      _M2Var：车右边的无刷电机
 * @param[out]     none
 * @retval         none
 */
void BLDC_SetSpeed(int16 _M1Var, int16 _M2Var)
{
    _M1Var *= 5;
    _M1Var = LIMIT_P(_M1Var, 500);
    _M2Var *= 5;
    _M2Var = LIMIT_P(_M2Var, 500);

    pwm_duty(PWMB_CH2_P75, 500 + _M1Var);
    pwm_duty(PWMB_CH1_P74, 500 + _M2Var);
}